﻿#include "urdfparser.h"
#include "models/command.h"
#include <QFile>


UrdfParser::UrdfParser(QObject *parent)
    : QObject(parent)
{
}

bool UrdfParser::parseFile(const QString& filename)
{
    QFile file(filename);
    if (!file.open(QIODevice::ReadOnly)) {
        m_strState = tr("打开文件失败：")+filename;
        return false;
    }

    QDomDocument doc;
    if (!doc.setContent(&file)) {
        m_strState = tr("文件解析失败：")+filename;
        file.close();
        return false;
    }
    file.close();

    QDomElement root = doc.documentElement();
    if (root.tagName() != "robot") {
        return false;
    }

    // 清除之前的数据
    m_joints.clear();
    m_links.clear();
    m_jointMap.clear();

    // 解析links
    QDomElement element = root.firstChildElement("link");
    while (!element.isNull()) {
        parseLink(element);
        element = element.nextSiblingElement("link");
    }

    // 解析joints
    element = root.firstChildElement("joint");
    while (!element.isNull()) {
        parseJoint(element);
        element = element.nextSiblingElement("joint");
    }

    updateTransforms();
    emit modelLoaded();
    return true;
}

bool UrdfParser::parseJoint(const QDomElement& element)
{
    Joint joint;
    joint.name = element.attribute("name");
    joint.type = element.attribute("type");
    joint.position = 0.0;

    QDomElement parent = element.firstChildElement("parent");
    if (!parent.isNull()) {
        joint.parent = parent.attribute("link");
    }

    QDomElement child = element.firstChildElement("child");
    if (!child.isNull()) {
        joint.child = child.attribute("link");
    }

    QDomElement origin = element.firstChildElement("origin");
    if (!origin.isNull()) {
        QString xyz = origin.attribute("xyz", "0 0 0");
        QStringList values = xyz.split(" ", Qt::SkipEmptyParts);
        if (values.size() == 3) {
            joint.origin = QVector3D(values[0].toFloat(),
                                   values[1].toFloat(),
                                   values[2].toFloat());
        }
    }

    QDomElement axis = element.firstChildElement("axis");
    if (!axis.isNull()) {
        QString xyz = axis.attribute("xyz", "0 0 1");
        QStringList values = xyz.split(" ", Qt::SkipEmptyParts);
        if (values.size() == 3) {
            joint.axis = QVector3D(values[0].toFloat(),
                                 values[1].toFloat(),
                                 values[2].toFloat());
        }
    }

    QDomElement limits = element.firstChildElement("limit");
    if (!limits.isNull()) {
        joint.limit_lower = limits.attribute("lower", "-3.14").toDouble();
        joint.limit_upper = limits.attribute("upper", "3.14").toDouble();
    }

    m_joints.append(joint);
    m_jointMap[joint.name] = m_joints.size() - 1;
    return true;
}

bool UrdfParser::parseLink(const QDomElement& element)
{
    Link link;
    link.name = element.attribute("name");
    link.transform.setToIdentity();
    link.visual_transform.setToIdentity();
    link.color = QVector3D(0.8f, 0.8f, 0.8f);  // 默认颜色

    QDomElement visual = element.firstChildElement("visual");
    if (!visual.isNull()) {
        QDomElement origin = visual.firstChildElement("origin");
        if (!origin.isNull()) {
            QString xyz = origin.attribute("xyz", "0 0 0");
            QStringList values = xyz.split(" ", Qt::SkipEmptyParts);
            if (values.size() == 3) {
                link.visual_origin = QVector3D(values[0].toFloat(),
                                             values[1].toFloat(),
                                             values[2].toFloat());
            }

            // 解析旋转
            QString rpy = origin.attribute("rpy", "0 0 0");
            QStringList rpyValues = rpy.split(" ", Qt::SkipEmptyParts);
            if (rpyValues.size() == 3) {
                QMatrix4x4 rot;
                rot.rotate(rpyValues[0].toFloat() * 180.0f / M_PI, 1, 0, 0);
                rot.rotate(rpyValues[1].toFloat() * 180.0f / M_PI, 0, 1, 0);
                rot.rotate(rpyValues[2].toFloat() * 180.0f / M_PI, 0, 0, 1);
                link.visual_transform = rot;
            }
        }

        QDomElement geometry = visual.firstChildElement("geometry");
        if (!geometry.isNull()) {
            QDomElement mesh = geometry.firstChildElement("mesh");
            if (!mesh.isNull()) {
                QString meshUri = mesh.attribute("filename");
                link.mesh_path = resolvePackagePath(meshUri);
            }
        }

        // 解析材质
        QDomElement material = visual.firstChildElement("material");
        if (!material.isNull()) {
            QDomElement color = material.firstChildElement("color");
            if (!color.isNull()) {
                QString rgba = color.attribute("rgba", "0.8 0.8 0.8 1.0");
                QStringList values = rgba.split(" ", Qt::SkipEmptyParts);
                if (values.size() >= 3) {
                    link.color = QVector3D(values[0].toFloat(),
                                         values[1].toFloat(),
                                         values[2].toFloat());
                }
            }
        }
    }

    m_links.append(link);
    return true;
}

void UrdfParser::updateJointPosition(const QString& name, double position)
{
    int index = m_jointMap.value(name, -1);
    if (index >= 0 &&  m_joints[index].position != position) {
        m_joints[index].position = position;
        updateTransforms();
        emit jointUpdated();
    }
}

QString UrdfParser::resolvePackagePath(const QString& uri)
{
    if (uri.startsWith("package://")) {
        QString relativePath = uri.mid(10);  // 移除 "package://"
        // 替换包名为实际路径
        int firstSlash = relativePath.indexOf('/');
        if (firstSlash != -1) {
            QString packageName = relativePath.left(firstSlash);
            QString filePath = relativePath.mid(firstSlash + 1);
            return m_packagePath + "/" + filePath;
        }
    }
    return uri;
}

QString UrdfParser::strState() const
{
    return m_strState;
}

void UrdfParser::updateTransforms()
{
    // 重置所有连杆的变换矩阵
    for (Link& link : m_links) {
        link.transform.setToIdentity();
    }

    // 更新每个关节的变换
    for (const Joint& joint : m_joints) {
        QMatrix4x4 transform;
        transform.setToIdentity();
        transform.translate(joint.origin);

        if (joint.type == "revolute" || joint.type == "continuous") {
            transform.rotate(joint.position * 180.0 / M_PI, joint.axis);
        } else if (joint.type == "prismatic") {
            transform.translate(joint.axis * joint.position);
        }

        // 更新子连杆的变换
        for (Link& link : m_links) {
            if (link.name == joint.child) {
                for (const Link& parentLink : m_links) {
                    if (parentLink.name == joint.parent) {
                        link.transform = parentLink.transform * transform;
                        break;
                    }
                }
                break;
            }
        }
    }
}
